#include <iostream>
#include <string>
#include <fstream>
#include <thread>
#include <deque>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <GeographicLib/LocalCartesian.hpp>





class recordPath2TUM
{
public:
    std::string root_dir = ROOT_DIR;
    const std::string GPSPath = std::string(std::string(ROOT_DIR)) + "log/Groud-truth.txt";  
    const std::string LLIOPath = std::string(std::string(ROOT_DIR)) + "log/LLIO.txt";
    const std::string FastLio2Path = std::string(std::string(ROOT_DIR)) + "log/FastLio2.txt";
    const std::string LIOSAMPath = std::string(std::string(ROOT_DIR)) + "log/LIOSAM.txt";
    const std::string FastLioSAMPath = std::string(std::string(ROOT_DIR)) + "log/FastLioSAM.txt";
    const std::string LegOdomPath = std::string(std::string(ROOT_DIR)) + "log/LegOdom.txt";
    const std::string IMUPath = std::string(std::string(ROOT_DIR)) + "log/IMU.txt";


    int reflashGPS = system((std::string("exec rm ") + GPSPath).c_str());
    int reflashLLIO = system((std::string("exec rm ") + LLIOPath).c_str());
    int reflashFastLio2 = system((std::string("exec rm ") + FastLio2Path).c_str());
    int reflashLIOSAM = system((std::string("exec rm ") + LIOSAMPath).c_str());
    int reflashFastLioSAM = system((std::string("exec rm ") + FastLioSAMPath).c_str());
    int reflashLegOdom = system((std::string("exec rm ") + LegOdomPath).c_str());
    int reflashIMU = system((std::string("exec rm ") + IMUPath).c_str());


    
    std::ofstream Path2TUMFiles;

    bool GPS_Init = false;    // GPS初始化接受数据标志位
    double origin_longitude;  // 起始点的经度
    double origin_latitude;   // 起始点的纬度
    double origin_altitude;   // 起始点的高度
    double latitude;      // 纬度
    double longitude;     // 经度
    double altitude;      // 高度
    double local_E;       // 东(x)
    double local_N;       // 北(y)
    double local_U;       // 天(z)

    ros::NodeHandle nh;
    ros::Subscriber subNavsatfix;
    ros::Subscriber subLLIO_odom;
    ros::Subscriber subFastLio2_odom;
    ros::Subscriber subLIOSAM_odom;
    ros::Subscriber subFastLioSAM_odom;
    ros::Subscriber subLegOdom_odom;
    ros::Subscriber subIMU;


    std::string navsatfix_Topic ="/navsatfix";
    std::string LLIO_odom_Topic = "/LLIO/Odometry";
    std::string FastLio2_odom_Topic = "/Odometry";
    std::string LIOSAM_odom_Topic = "/lio_sam/mapping/odometry";
    std::string FastLioSAM_odom_Topic = "/FastLioSAM/Odometry";
    std::string LegOdom_odom_Topic = "/leg_odom";
    std::string IMU_Topic = "/xsens/imu/data";


    recordPath2TUM()
    {
        subNavsatfix = nh.subscribe<sensor_msgs::NavSatFix>(navsatfix_Topic, 1000, &recordPath2TUM::navsatfix_handle, this, ros::TransportHints().tcpNoDelay());
        subLLIO_odom = nh.subscribe<nav_msgs::Odometry>(LLIO_odom_Topic, 1000, &recordPath2TUM::LLIO_odom_handle, this, ros::TransportHints().tcpNoDelay());
        subFastLio2_odom = nh.subscribe<nav_msgs::Odometry>(FastLio2_odom_Topic, 1000, &recordPath2TUM::FastLio2_odom_handle, this, ros::TransportHints().tcpNoDelay());
        subLIOSAM_odom = nh.subscribe<nav_msgs::Odometry>(LIOSAM_odom_Topic, 1000, &recordPath2TUM::LIOSAM_odom_handle, this, ros::TransportHints().tcpNoDelay());
        subFastLioSAM_odom = nh.subscribe<nav_msgs::Odometry>(FastLioSAM_odom_Topic, 1000, &recordPath2TUM::FastLioSAM_odom_handle, this, ros::TransportHints().tcpNoDelay());
        subLegOdom_odom = nh.subscribe<nav_msgs::Odometry>(LegOdom_odom_Topic, 1000, &recordPath2TUM::LegOdom_odom_handle, this, ros::TransportHints().tcpNoDelay());
        subIMU = nh.subscribe<sensor_msgs::Imu>(IMU_Topic, 1000, &recordPath2TUM::IMU_handle, this, ros::TransportHints().tcpNoDelay());

    }



    /*************************************************************
     * @brief 订阅GPS的回调函数
     * 
     * @param msgIn 
     ************************************************************/
    void navsatfix_handle(const sensor_msgs::NavSatFix::ConstPtr& msgIn)
    {
        sensor_msgs::NavSatFix data = *msgIn;
        double time;
        double lat, lon, alt;

        time = data.header.stamp.toSec();   // 时间戳
        lat = data.latitude;    // 纬度
        lon = data.longitude;   // 经度
        alt = data.altitude;    // 高度

        if (!GPS_Init)
        {
            InitOriginPosition(lat, lon, alt);           
            saveData2TUM(GPSPath, time, 0, 0, 0, 0, 0, 0, 1);
            
            ROS_INFO("\033[1;31m GPS OriginPosition Inited.\033[0m");
            GPS_Init = true;
            return;
        }

        UpdateXYZ(lat, lon, alt);
        saveData2TUM(GPSPath, time, local_E, local_N, local_U, 0, 0, 0, 1);
    }


    void LLIO_odom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;

        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(LLIOPath, time, x, y, z, qx, qy, qz, qw);
    }

    void FastLio2_odom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;
        
        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(FastLio2Path, time, x, y, z, qx, qy, qz, qw);
    }

    void LIOSAM_odom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;
        
        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(LIOSAMPath, time, x, y, z, qx, qy, qz, qw);
    }

    void FastLioSAM_odom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;
        
        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(FastLioSAMPath, time, x, y, z, qx, qy, qz, qw);
    }

    void LegOdom_odom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;
        
        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(LegOdomPath, time, x, y, z, qx, qy, qz, qw);
    }

    void IMU_handle(const sensor_msgs::Imu::ConstPtr& msgIn)
    {
        sensor_msgs::Imu data = *msgIn;
        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = 0;
        y = 0;
        z = 0;
        qx = data.orientation.x;
        qy = data.orientation.y;
        qz = data.orientation.z;
        qw = data.orientation.w;

        saveData2TUM(IMUPath, time, x, y, z, qx, qy, qz, qw);
    }
    
    
    
    
    /*************************************************************
     * @brief 初始化原点
     * 
     * @param latitude 维度
     * @param longitude 经度
     * @param altitude 高度
     ************************************************************/
    void InitOriginPosition(double latitude, double longitude, double altitude)
    {
        // 重置原点 (0, 0, 0)
        geo_converter.Reset(latitude, longitude, altitude);
        origin_latitude = latitude;
        origin_longitude = longitude;
        origin_altitude = altitude;
    }

    /*************************************************************
     * @brief 更新ENU坐标
     * 将经纬高转换成米制坐标 (x, y, z)
     * 
     * @param latitude 维度
     * @param longitude 经度
     * @param altitude 高度
     ************************************************************/
    void UpdateXYZ(double latitude, double longitude, double altitude)
    {
        // Forward (lat, lon, alt, x, y, z)函数就是把经纬高转换为ENU，前三个传入，后三个传出
        geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U); 
    }


    /************************************************************
     * @brief 将数据写入成TUM的轨迹数据格式文件
     * 
     * @param file 文件路径
     * @param time 时间戳
     * @param x 
     * @param y 
     * @param z 
     * @param qx 
     * @param qy 
     * @param qz 
     * @param qw 
     ***********************************************************/
    void saveData2TUM(std::string file, double time, double x, double y, double z, double qx, double qy, double qz, double qw)
    {
        Path2TUMFiles.open(file, std::ios::app);
        if (Path2TUMFiles.is_open())
        {
            Path2TUMFiles << std::fixed << time
                << " " << x
                << " " << y
                << " " << z
                << " " << qx
                << " " << qy
                << " " << qz
                << " " << qw
                << std::endl;
            Path2TUMFiles.close();
        }
    }
    
    


    void pathDataProcess()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            rate.sleep();
        }
        
    }


private:
    GeographicLib::LocalCartesian geo_converter;
};








int main(int argc, char** argv)
{
    ros::init(argc, argv, "recordPath2TUM");

    recordPath2TUM MO;

    ROS_INFO("\033[1;32m----> record path data Procee Started.\033[0m");

    std::thread recordPath2TUMThread(&recordPath2TUM::pathDataProcess, &MO);

    ros::spin();

    recordPath2TUMThread.join();

    return 0;
}